package planesimulator.plane;

import com.jme.math.*;
import com.jmex.terrain.TerrainPage;
import planesimulator.game.ModelLoader;
import planesimulator.plane.PlaneMath.*;


//import com.jme.scene.*;
import planesimulator.plane.wing.Ailerions;
import planesimulator.plane.wing.HorizontalWing;
import planesimulator.plane.wing.coefficients.StandartDragCoefficient;
import planesimulator.plane.wing.coefficients.StandartLiftCoefficient;
import planesimulator.plane.wing.coefficients.TailLiftCoefficient;
import planesimulator.plane.wing.VerticalWing;
import planesimulator.plane.wing.Wing;
import planesimulator.plane.wing.coefficients.ValueTable;

/**
 * 
 * Class that simulates Cessna Citation 525 aircraft.
 * @author Jakub Klimek
 * 
 * 
 */
public class CessnaCitation extends AbstractPlane {

  /** Speed vector */
  protected Vector3f speed = new Vector3f(150, 0, 0);
  
  Vector3f plane_speed;
  
  /** Plane's angular speed */
  protected Vector3f omega = new Vector3f(0, 0, 0);
  
  /** Plane's inertia Matrix */
  protected final Matrix3f interia = 
      new Matrix3f(69885, 0, 0, 0, 97307, 0, 0, 0, 58954);
  
  /** Inversed inertia matrix */
  protected Matrix3f inverseInteria;
  
  /** Mass of the plane [kg]*/
  protected final float mass = 4700f;
  
  /** Maximal engines power [N] */
  protected final float enginesMaximalPower = 15000f;
  
  /** Angle of attack on vertical wings */
  protected float vertical_angle_of_attack = 0;
  
  /** Angle of attack on horizontal wings */
  protected float horizontal_angle_of_attack = 0;
  
  /** Plane's coordinate to world coordinate matrix */
  protected Matrix3f worldCoord;
  
  /** World coordinate to plane coordinate matrix */
  protected Matrix3f planeCoord;
  
  /** Main wing */
  protected Wing main_wing;
  
  /** Vertical stabilizer wing */
  protected Wing vertical_stabilizer;
  
  /** Horizontal stabilizer wing */
  protected Wing horizontal_stabilizer;
  
  /** Elevator */
  protected Wing elevator;
  
  /** Rudder */
  protected Wing rudder;
  
  /** Ailerion */
  protected Wing ailerion;
  
  /** Position of the left engine */
  protected final Vector3f left_engine_pos = new Vector3f(-2.15f, 1f, -1.65f);
  
  /** Position of the right engine */
  protected final Vector3f right_engine_pos = new Vector3f(-2.15f, 1f, 1.65f);

  /** Position of left rear wheel **/
  protected final Vector3f right_rear_gear = new Vector3f(0,-2f,-3f);
  
  /** Terrain class needed to evaluate distance from gound **/
  TerrainPage terrain;
  
  /** Value table to get air density at current height */
  protected ValueTable density_table = new ValueTable(15,
      new float[]{
        0.0f,
        1000.0f,
        2000.0f,
        3000.0f,
        4000.0f,
        5000.0f,
        6000.0f,
        7000.0f,
        8000.0f,
        9000.0f,
        10000.0f,
        11000.0f,
        12000.0f,
        13000.0f,
        14000.0f,
        15000.0f
      }, new float[]{
        1.225f,
        1.112f,
        1.007f,
        0.909f,
        0.819f,
        0.736f,
        0.660f,
        0.590f,
        0.526f,
        0.467f,
        0.414f,
        0.365f,
        0.312f,
        0.267f,
        0.228f,
        0.195f,
      });
  
  protected ModelLoader model;
  
  /** Constructor */
  public CessnaCitation(TerrainPage terrain) {
    super("Cessna Citation 525",terrain);
    this.terrain = terrain;
    inverseInteria = interia.invert();
    main_wing =
        new HorizontalWing(
        "Main wing",
        11.63f * 2f, this,
        new StandartLiftCoefficient(), new StandartDragCoefficient(),
        new Vector3f(0f, 0f, 0));
    elevator = new HorizontalWing(
        "Elevator",
        1.6f * 2, this,
        new TailLiftCoefficient(), new StandartDragCoefficient(),
        new Vector3f(-6.14f, 2.68f, 0f));
    horizontal_stabilizer =
        new HorizontalWing(
        "Horizontal stabilizer",
        1.6f * 2, this,
        new TailLiftCoefficient(), new StandartDragCoefficient(),
        new Vector3f(-5.2f, 2.68f, 0f));
    ailerion =
        new Ailerions(
        1.84f * 2f, this,
        new TailLiftCoefficient(), new StandartDragCoefficient(),
        new Vector3f(-1f, 0, 6.36f));

    vertical_stabilizer = new VerticalWing(
        "Vertical stabilizer",
        2.05f, this,
        new TailLiftCoefficient(), new StandartDragCoefficient(),
        new Vector3f(-5.06f, 1.91f, 0.00f));
    rudder = new VerticalWing(
        "Rudder",
        1.07f, this,
        new TailLiftCoefficient(), new StandartDragCoefficient(),
        new Vector3f(-6.12f, 1.91f, 0.00f));

    model = new ModelLoader(this);
  }

  void updateSteer(float time) {
    float coeff_vertical = 0.35f;
    float coeff_horizontal = 0.55f;


    float vertical_back = time * coeff_vertical;
    float horizontal_back = time * coeff_horizontal;

    if (left_right >= 0) {
      if (left_right >= horizontal_back) {
        left_right -= horizontal_back;
      } else {
        left_right = 0;
      }
    } else if (left_right <= -horizontal_back) {
      left_right += horizontal_back;
    } else {
      left_right = 0;
    }
    if (up_down >= 0) {
      if (up_down >= vertical_back) {
        up_down -= vertical_back;
      } else {
        up_down = 0;
      }
    } else if (up_down <= -vertical_back) {
      up_down += vertical_back;
    } else {
      up_down = 0;
    }
  }

  Vector3f estimatedSpeed(Vector3f position) {
    Vector3f new_position = new Vector3f();
    PlaneMath.vectoutprod(omega, position, new_position);
    return plane_speed.subtract(position).add(new_position);
  }

  float horizontalAngleOfAttack(Vector3f speed) {
    return (float) -Math.toDegrees(Math.atan2(speed.y, speed.x));
  }

  float verticalAngleOfAttack(Vector3f speed) {
    return (float) -Math.toDegrees(Math.atan2(speed.z, speed.x));
  }

  void setHorizontalAngleOfAttack(Wing w) {
    w.angle_of_attack =
        horizontalAngleOfAttack(
        estimatedSpeed(w.position));

  }

  void setVerticalAngleOfAttack(Wing w) {
    w.angle_of_attack =
        verticalAngleOfAttack(
        estimatedSpeed(w.position));
  }

  /** Method changes position and rotation of object
   * every next frame.
   * 
   * @param time Time interval between consecutive frames
   */
  public void update(float time) {
    // updating streer, position and rotation
    updateSteer(time);
  
    // Updating plane position
    Vector3f tr = this.getLocalTranslation();
    Vector3f mv = speed.mult(time);
    Vector3f v = tr.add(mv);
    this.setLocalTranslation(v);
    
    // Setting air density
    float l = speed.length();
    density = l * l * 0.5f * density_table.interpolate(tr.y);

    //Updating plane rotation
    Quaternion rotation = this.getLocalRotation();
    Quaternion ro = new Quaternion();
    ro.fromAngles(time * omega.x, time * omega.y, time * omega.z);
    PlaneMath.quatprod(rotation, ro, rotation);
    this.setLocalRotation(rotation);

    worldCoord = rotation.toRotationMatrix();
    planeCoord = new Quaternion(-rotation.x, -rotation.y, -rotation.z, 
        rotation.w).toRotationMatrix();

    // Calculation angles of attack
    plane_speed = planeCoord.mult(speed);

    // Horizontal angle of attack
    horizontal_angle_of_attack = horizontalAngleOfAttack(plane_speed);
    main_wing.angle_of_attack = horizontal_angle_of_attack;
    ailerion.angle_of_attack = horizontal_angle_of_attack;
    setHorizontalAngleOfAttack(horizontal_stabilizer);
    setHorizontalAngleOfAttack(elevator);
    elevator.angle_of_attack += up_down - 1.9f;
    //System.out.println(up_down-1.9f+" "+l*3.6f);

    // Vertical alnble of attack
    vertical_angle_of_attack = verticalAngleOfAttack(plane_speed);
    setVerticalAngleOfAttack(vertical_stabilizer);
    setVerticalAngleOfAttack(rudder);
    rudder.angle_of_attack += rudder_pos;

    // Calculating total torque and force
    Vector3f force = new Vector3f(0, 0, 0); // forces on the plane
    Vector3f torque = new Vector3f(0, 0, 0);  // torque of the plane

    calcuateEngine(force, torque);
    calcuateWings(force, torque);
    calcuateActiveWings(force, torque);
    calcuateStabilizers(force, torque);
    calculateRudder(force, torque);
    calculateGears(force,torque);
    addGravity(force);

    // Updating speed and angular speed
    Vector3f wxMw = new Vector3f();
    wxMw = interia.mult(omega);
    PlaneMath.vectoutprod(omega, wxMw, wxMw);
    torque.subtractLocal(wxMw);
    Vector3f alpha = new Vector3f();
    alpha = inverseInteria.mult(torque);
    omega.addLocal(alpha.mult(time));

    force = worldCoord.mult(force);
    force = force.mult(time / mass);
    speed = speed.add(force);
  }

  void addGravity(Vector3f f) {
    Vector3f gravity = new Vector3f(0, -mass * 10, 0);
    f.addLocal(planeCoord.mult(gravity));
  }

  void calcuateEngine(Vector3f force, Vector3f torque) {
    float fv = engine_power * enginesMaximalPower;
    force.x += fv;
    Vector3f let = new Vector3f(fv / 2, 0f, 0f);
    Vector3f ret = new Vector3f(fv / 2, 0f, 0f);
    PlaneMath.vectoutprod(left_engine_pos, let, let);
    torque.addLocal(let);
    PlaneMath.vectoutprod(right_engine_pos, ret, ret);
    torque.addLocal(ret);

  }

  void calcuateWings(Vector3f force, Vector3f torque) {
    main_wing.calculateForceOnWing(force);
  }

  void calcuateActiveWings(Vector3f force, Vector3f torque) {
    ailerion.calculateForcesOnWing(force, torque);
    elevator.calculateForcesOnWing(force, torque);
  }

  void calcuateStabilizers(Vector3f force, Vector3f torque) {
    vertical_stabilizer.calculateForcesOnWing(force, torque);
    horizontal_stabilizer.calculateForcesOnWing(force, torque);
  }

  void calculateRudder(Vector3f force, Vector3f torque) {
    rudder.calculateForcesOnWing(force, torque);

  }
  
  void calculateGears(Vector3f force, Vector3f torque){
    Vector3f pp = getLocalTranslation();
    Vector3f gearpos = pp.add(worldCoord.mult(right_rear_gear));
    float height = terrain.getHeight(gearpos);
//    System.out.println(height);
  }
}
